import rospy
from pi_car.srv import StringRequest, StringResponse, String
from std_srvs.srv import Empty, EmptyRequest, EmptyResponse

from nlink_parser.msg import LinktrackNodeframe2
import numpy as np

import time


class CarControllerBase:
    def __init__(self):
        rospy.wait_for_service("/pi_car/controller/brake")
        rospy.wait_for_service("/pi_car/controller/set_yaw")
        rospy.wait_for_service("/pi_car/controller/set_speed")

        self.brake_srv = rospy.ServiceProxy("/pi_car/controller/brake", Empty)
        self.yaw_srv = rospy.ServiceProxy("/pi_car/controller/set_yaw", String)
        self.speed_srv = rospy.ServiceProxy("/pi_car/controller/set_speed", String)

        print("==== init car controller ==== ")

    def brake(self):
        """set speed to 0"""
        self.brake_srv(EmptyRequest())

    def set_yaw(self, yaw: float):
        """set yaw (degree)"""
        self.yaw_srv(str(yaw))

    def set_speed(self, speed_ratio: float):
        """set speed ratio, [0, 1]"""
        self.speed_srv(str(speed_ratio))


class TrackTest(CarControllerBase):
    def __init__(self) -> None:
        super().__init__()

        # uwb data
        self.pos = None
        self.eop = None

        self.uwb_pos_sub = rospy.Subscriber(
            "/nlink_linktrack_nodeframe2",
            LinktrackNodeframe2,
            self.on_uwb_data
        )

        print("==== init track test ===")


        # task
        task = rospy.Timer(
            rospy.Duration(0.1),
            callback = self.task
        )


    def on_uwb_data(self, data: LinktrackNodeframe2):
        self.pos = data.pos_3d
        self.eop = np.mean(data.eop_3d)

        print(self.pos, self.eop)

    def task(self, timer):
        # put task code here
        pass


if __name__ == "__main__":
    rospy.init_node("car_test")

    test = TrackTest()
    
    rospy.spin()


